#include "RandomRespSolver.h"

#include <algorithm>
#include <iostream>
#include <map>

#include "Utility/Logging/SGLogger.h"

#include "DataStructure/Input/Solver/DOFDirections.h"
#include "DataStructure/Input/Solver/Subcase.h"

#include "DBManager/IService/Input/Dynamic/IRANDPSService.h"
#include "DBManager/IService/Input/Dynamic/IRCROSSService.h"
#include "DBManager/IService/Input/Parameter/IParameterService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"
#include "DBManager/IService/Input/Table/ITABRND1Service.h"

#include "SGFem/Pre/Assembly/Assembly.h"

#include "SGFem/ElementCalculator/Factory/ElementCalculatorFactory.h"
#include "SGFem/Task/Kernel/MModel.h"

using SG::Algebra::Complex_t;
using namespace SG::DataStructure::Common;
using SG::DataStructure::FEM::SOLType;
using SG::DataStructure::FEM::TABRND1Data;
using namespace SG::DBManager;
using namespace std::literals::string_literals;

SG::Algebra::Real_t
RandomRespSolver::TrapzIntegral (SG::Algebra::Real_t x0, SG::Algebra::Real_t x1, SG::Algebra::Real_t y0, SG::Algebra::Real_t y1)
{
    return 0.5 * (y0 + y1) * (x1 - x0);
}

SG::Algebra::Real_t
RandomRespSolver::LogLogIntegral (SG::Algebra::Real_t x0, SG::Algebra::Real_t x1, SG::Algebra::Real_t y0, SG::Algebra::Real_t y1)
{
    SG::Algebra::Real_t alpha = std::log10 (y1 / y0) / std::log10 (x1 / x0);
    if (std::abs (alpha + 1.0) < 1e-8)
    {
        return x0 * y0 * std::log (x1 / x0);
    }
    else
    {
        return y0 / ((1.0 + alpha) * std::pow (x0, alpha)) * (std::pow (x1, alpha + 1.0) - std::pow (x0, alpha + 1.0));
    }
}

void RandomRespSolver::ComputeRMSCurve (const std::vector<SG::Algebra::Real_t>& freq,
                                        const std::vector<SG::Algebra::Real_t>& psd,
                                        std::vector<SG::Algebra::Real_t>&       rms_curve,
                                        const std::string&                      IntegralMethod)
{
    rms_curve.clear ();
    rms_curve.resize (freq.size (), 0.0);
    SG::Algebra::Real_t temp = 0.0;
    for (size_t i = 1; i < freq.size (); ++i)
    {
        SG::Algebra::Real_t integral_result1 = 0.0;
        if (IntegralMethod == "Linear")
        {
            //  //trapz积分
            integral_result1 = TrapzIntegral (freq[i - 1], freq[i], psd[i - 1], psd[i]);
        }
        else
        {
            // loglog积分
            integral_result1 = LogLogIntegral (freq[i - 1], freq[i], psd[i - 1], psd[i]);
        }

        temp += integral_result1;
        rms_curve[i] = std::sqrt (temp);
    }
}

void RandomRespSolver::ComputeNumeberzero (const std::vector<SG::Algebra::Real_t>& freq,
                                           const std::vector<SG::Algebra::Real_t>& psd,
                                           SG::Algebra::Real_t&                    numberzero,
                                           const std::string&                      IntegralMethod)
{
    std::vector<SG::Algebra::Real_t> psd1 (freq.size ());
    for (size_t i = 0; i < freq.size (); ++i)
    {
        psd1[i] = freq[i] * freq[i] * psd[i];  // (2πf)^2 * Syy
    }

    SG::Algebra::Real_t temp  = 0.0;
    SG::Algebra::Real_t temp1 = 0.0;
    for (size_t i = 1; i < freq.size (); ++i)
    {
        SG::Algebra::Real_t integral_result  = 0.0;
        SG::Algebra::Real_t integral_result1 = 0.0;

        if (IntegralMethod == "Linear")
        {
            // trapz积分
            integral_result  = TrapzIntegral (freq[i - 1], freq[i], psd[i - 1], psd[i]);
            integral_result1 = TrapzIntegral (freq[i - 1], freq[i], psd1[i - 1], psd1[i]);
        }
        else
        {
            integral_result  = LogLogIntegral (freq[i - 1], freq[i], psd[i - 1], psd[i]);
            integral_result1 = LogLogIntegral (freq[i - 1], freq[i], psd1[i - 1], psd1[i]);
        }

        temp += integral_result;
        temp1 += integral_result1;
    }
    numberzero = std::sqrt (temp1 / temp);
}

void RandomRespSolver::Solve (const SG::DBManager::DBServiceFactorySP dbServiceFactory, SG::DataStructure::Common::IdArray subcaseIds)
{
    std::cout << "random resp solve \n";
    // 查询工况参数
    auto para            = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (subcaseIds[0]);
    auto solverParameter = dbServiceFactory->get<IParameterService> ()->GetSolverParameter ();

    SG::DataStructure::FEM::Subcase subcase (para);

    // construct model

    Kernel::MModel model (SOLType::SOL_SEMFREQ, dbServiceFactory);
    model.Initialize (subcaseIds);
    // 读取积分方法参数
    std::string                                     IntergralMethod = solverParameter.GetValue ("RMSINT"s).LookupOrDefault ("DEFAULT"s);
    auto                                            RCROSS          = subcase.GetRCROSSId ();
    std::vector<SG::DataStructure::FEM::RCROSSData> rcrossDatas;
    // Id                                              sid1 = 1000;
    dbServiceFactory->get<IRCROSSService> ()->FindBySId (RCROSS, _OUT rcrossDatas);
    // 获取计算频率点
    std::vector<Real> w;
    if (Status::STATUS_Failure == model.QueryFrenqencePoint (subcase.GetFrequencyId (), _OUT w))
    {
        return;
    }

    const auto nSub = subcaseIds.size ();

    Id                                              sid = subcase.GetRANDPSId ();
    std::vector<SG::DataStructure::FEM::RANDPSData> randpsDatas;
    dbServiceFactory->get<IRANDPSService> ()->FindBySId (sid, _OUT randpsDatas);

    // Read S0
    SG::Algebra::ComplexMat_t S0 (nSub, nSub);
    SG::Algebra::Matrix       Ti_real (nSub, nSub);
    SG::Algebra::Matrix       Ti_imag (nSub, nSub);
    std::set<Id>              TiSet;
    S0.setZero ();
    for (const auto& randpsI : randpsDatas)
    {
        // subcase id 不一定连续
        auto       it = std::find (subcaseIds.begin (), subcaseIds.end (), randpsI.m_J);
        const auto J  = std::distance (subcaseIds.begin (), it);

        const auto it2 = std::find (subcaseIds.begin (), subcaseIds.end (), randpsI.m_K);
        const auto K   = std::distance (subcaseIds.begin (), it2);
        if (J > K)
        {
            SGError << "invalid randps data\n";
            continue;
        }
        if (randpsI.m_X != 0.0)  // 实部数据
        {
            // 累加实部
            S0 (J, K) += Complex_t (randpsI.m_X, 0.0);
            // 存储实部对应的表ID
            Ti_real (J, K) = randpsI.m_TID;
            TiSet.insert (randpsI.m_TID);
        }

        if (randpsI.m_Y != 0.0)  // 虚部数据
        {
            // 累加虚部
            S0 (J, K) += Complex_t (0.0, randpsI.m_Y);
            // 存储虚部对应的表ID
            Ti_imag (J, K) = randpsI.m_TID;
            TiSet.insert (randpsI.m_TID);
        }
    }

    // Get all TABRND1
    const auto                ITABRND1Resp = dbServiceFactory->get<ITABRND1Service> ();
    std::map<Id, TABRND1Data> TABNDSet;
    TABRND1Data               table;
    for (const auto& tI : TiSet)
    {
        ITABRND1Resp->FindById (tI, _OUT table);
        TABNDSet[tI] = table;
    }
    TiSet.clear ();

    SG::Algebra::ComplexMat_t                     Sxx (nSub, nSub);
    SG::Algebra::ComplexMat_t                     Hnk;
    SG::Algebra::Vec_t                            CurrentDisplacementPSD (nSub);
    SG::Algebra::ComplexVec_t                     CROSSPSD (nSub - 1, nSub - 1);
    std::vector<std::vector<SG::Algebra::Real_t>> ALLDisplacementPSD (nSub, std::vector<SG::Algebra::Real_t> (w.size (), 0.0));
    int                                           freqIndex = 0;
    for (const auto& f : w)
    {
        // 获取当前频率的频响矩阵
        model.GetFreaRespMatrix (f, subcaseIds, _OUT Hnk);

        // compute S
        Sxx.setZero ();
        for (std::size_t j = 0; j < nSub; ++j)
        {
            for (std::size_t k{ j }; k < nSub; ++k)
            {
                Real real_part = 0.0;
                Real imag_part = 0.0;
                // 处理实部：查找实部对应的表ID并插值
                if (Ti_real (j, k) != 0)
                {
                    const auto it_real = TABNDSet.find (Ti_real (j, k));
                    if (it_real != TABNDSet.end ())
                    {
                        const auto fi_real = it_real->second.Interpolate (f);
                        real_part          = S0 (j, k).real () * fi_real;
                    }
                    else
                    {
                        SGFatal << "can't find table for real part, TID = " << Ti_real (j, k);
                    }
                }

                // 处理虚部：查找虚部对应的表ID并插值
                if (Ti_imag (j, k) != 0)
                {
                    const auto it_imag = TABNDSet.find (Ti_imag (j, k));
                    if (it_imag != TABNDSet.end ())
                    {
                        const auto fi_imag = it_imag->second.Interpolate (f);
                        imag_part          = S0 (j, k).imag () * fi_imag;
                    }
                    else
                    {
                        SGFatal << "can't find table for imag part, TID = " << Ti_imag (j, k);
                    }
                }

                // 组合实部和虚部
                Sxx (j, k) = Complex_t (real_part, imag_part);

                // 填充下三角（共轭对称）
                if (j != k)
                {
                    Sxx (k, j) = std::conj (Sxx (j, k));
                }
            }
        }

        auto Syy = Hnk * Sxx * Hnk.adjoint ();
        for (int i = 0; i < Syy.rows (); ++i)
        {
            CurrentDisplacementPSD[i]        = Syy (i, i).real ();
            ALLDisplacementPSD[i][freqIndex] = Syy (i, i).real ();
        }

        std::vector<SG::DataStructure::Post::CROSS_PSDData> crossDatas;

        if (!rcrossDatas.empty ())
        {
            auto&            assembly = SG::FEMSolver::AssemblyEigen::GetInstance ();
            std::vector<int> nodeGlobalDofIds (6);

            for (const auto& rc : rcrossDatas)
            {
                // 1) 计算第一个节点/分量对应的约束后自由度索引
                const auto dof1 = rc.GetCompDirection1 ();
                assembly.ComputeGlobalDOFId (rc.m_ID1, { dof1 }, _OUT nodeGlobalDofIds);
                assert (1 == nodeGlobalDofIds.size ());
                const auto globalId1 = nodeGlobalDofIds[0];

                // 2) 计算第二个节点/分量对应的约束后自由度索引
                const auto dof2 = rc.GetCompDirection2 ();
                assembly.ComputeGlobalDOFId (rc.m_ID2, { dof2 }, _OUT nodeGlobalDofIds);
                assert (1 == nodeGlobalDofIds.size ());
                const auto globalId2 = nodeGlobalDofIds[0];

                SG::DataStructure::Post::CROSS_PSDData crossData;
                crossData.m_RCROSS = RCROSS;
                memcpy (crossData.m_RTYPE1, "DISP", 4);
                memcpy (crossData.m_RTYPE2, "DISP", 4);
                crossData.m_ID1    = rc.m_ID1;
                crossData.m_ID2    = rc.m_ID2;
                crossData.m_COMP1  = rc.m_COMP1;
                crossData.m_COMP2  = rc.m_COMP2;
                crossData.m_RCPSDF = Syy (globalId1, globalId2).real ();
                crossData.m_ICPSDF = Syy (globalId1, globalId2).imag ();
                crossDatas.push_back (crossData);
            }
            model.SetCrossPSD (crossDatas, sid, f);
        }

        // CurrentDisplacementPSD需要存储在h5的Displacement里

        model.SetDisplacement (CurrentDisplacementPSD, sid, f, Kernel::MModel::RandomResultType::PSD);
        ++freqIndex;
    }
    std::vector<std::vector<SG::Algebra::Real_t>> RMScurve (nSub, std::vector<SG::Algebra::Real_t> (w.size (), 0.0));
    SG::Algebra::Vec_t                            NumberZeroCrossing (nSub);
    SG::Algebra::Vec_t                            lastValues (nSub);
    for (size_t i = 0; i < nSub; ++i)
    {
        ComputeRMSCurve (w, ALLDisplacementPSD[i], RMScurve[i], IntergralMethod);
        lastValues[i]                    = RMScurve[i].back ();  // 提取每列的最后一个值
        SG::Algebra::Real_t ZeroCrossing = 0.0;
        ComputeNumeberzero (w, ALLDisplacementPSD[i], ZeroCrossing, IntergralMethod);
        NumberZeroCrossing[i] = ZeroCrossing;
    }
    std::cout << "rms = \n" << lastValues << std::endl;
    std::cout << "过零次数 = \n" << NumberZeroCrossing << std::endl;

    model.SetDisplacement (lastValues, sid, 0, Kernel::MModel::RandomResultType::AVG);

    model.SetDisplacement (NumberZeroCrossing, sid, 0, Kernel::MModel::RandomResultType::ZERO);

    SG::Algebra::Vec_t crms (nSub);
    for (size_t f = 0; f < w.size () - 1; ++f)
    {
        // 当前频率下所有n个RMS值

        // 提取当前频率下的所有RMS值
        for (size_t i = 0; i < nSub; ++i)
        {
            crms[i] = RMScurve[i][f];
        }
        std::cout << "RMS曲线 = \n" << crms << std::endl;

        model.SetDisplacement (crms, sid, w[f], Kernel::MModel::RandomResultType::AVGSUM);
    }
}